#ifdef linux

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <linux/videodev2.h>

#include "get_yuv.h"

#define HEIGHT	240
#define WIDTH	320
#define DEV	"/dev/video0"
#define IMG	"img.yuv"

#define VBUF_COUNT	8

static int vd = -1;
static void *image_data[VBUF_COUNT] = {NULL};
static struct v4l2_buffer vbuf[VBUF_COUNT]; // Buffer's attribute for a fream in the driver

int init_camera(const char *dev_name, int width, int height)
{
	int ret;
	int i, cnt;
	struct v4l2_capability cap; // Camera's fix attribute
	struct v4l2_format fmt; // Set image's attribute
	struct v4l2_requestbuffers req; // Request buffers' command
	struct v4l2_control ctrl; // Auto focus to use

	vd =  open(dev_name, O_RDWR);
	if (vd < 0) {
		perror("Open video device");
		ret = vd;
		goto err_0;
	}

	ret = ioctl(vd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0) {
		perror("Query camera's capability");
		goto err_1;
	}

	memset(&fmt, 0, sizeof(fmt));
	fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width       = width;
	fmt.fmt.pix.height      = height;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
	fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
	ret = ioctl(vd, VIDIOC_S_FMT, &fmt);
	if (ret < 0) {
		perror("Set image's attribute");
		goto err_1;
	}

	memset(&req, 0, sizeof(req));
	req.count		= VBUF_COUNT;
	req.type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory		= V4L2_MEMORY_MMAP;
	ret = ioctl(vd, VIDIOC_REQBUFS, &req);
	if ((ret < 0) || (req.count < 1)) {
		perror("Request buffers");
		goto err_1;
	}

	for (i = 0; i < VBUF_COUNT; i++) {
		memset(&vbuf[i], 0, sizeof(vbuf[i]));
		vbuf[i].type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
		vbuf[i].memory	= V4L2_MEMORY_MMAP;
		vbuf[i].index	= i;
		ret = ioctl (vd, VIDIOC_QUERYBUF, &vbuf[i]);
		if (ret < 0) {
			perror("Set buffer's attribute");
			goto err_1;
		}

		image_data[i] = mmap(NULL, vbuf[i].length, PROT_READ, MAP_SHARED, vd, vbuf[i].m.offset);
		if (image_data[i] == NULL) {
			perror("Mmap");
			goto err_1;
		}

		ret = ioctl(vd, VIDIOC_QBUF, &vbuf[i]);
		if (ret < 0) {
			perror("Enqueue buffer");
			goto err_2;
		}

		ret = ioctl(vd, VIDIOC_STREAMON, &vbuf[i].type);
		if (ret < 0) {
			perror("Stream on");
			goto err_2;
		}
	}

	ctrl.id = V4L2_CID_FOCUS_AUTO;
	ctrl.value = 1;
	ioctl(vd, VIDIOC_S_CTRL, &ctrl);

	return ret;

err_2:
	for (cnt = 0; cnt < i; cnt++) { 
		munmap(image_data[cnt], vbuf[i].length);
		image_data[cnt] = NULL;

		ret = ioctl(vd, VIDIOC_STREAMOFF, &vbuf[i].type);
		if (ret < 0) {
            perror("Stream off");
		}
	}
err_1:
	close(vd);
	vd = -1;
err_0:
	return ret;
}

int get_frame(void *buf, unsigned int bufsize)
{
	int ret;
	fd_set fds;
	struct timeval tv;
    unsigned int copy_size;
	struct v4l2_buffer tmp_vbuf = {
		.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE,
		.memory	= V4L2_MEMORY_MMAP,
	};

	FD_ZERO(&fds);
	FD_SET(vd, &fds);
	tv.tv_sec  = 2;
	tv.tv_usec = 0;
	ret = select(vd + 1, &fds, NULL, NULL, &tv);
	if (ret < 0) {
		perror("Select");
		goto err_1;
	} else if (ret == 0) {
		perror("Timeout");
		goto err_1;
	} else {
		ret = ioctl(vd, VIDIOC_DQBUF, &tmp_vbuf);
		if (ret < 0) {
			perror("Dequeue buffer");
			goto err_1;
		}

		copy_size = bufsize > tmp_vbuf.length ? tmp_vbuf.length : bufsize;
	 	memset(buf, 0, copy_size);
		memcpy(buf, image_data[tmp_vbuf.index], copy_size);
		
		ret = ioctl(vd, VIDIOC_QBUF, &tmp_vbuf);
		if (ret < 0) {
			perror("Enqueue buffer");
		}
		
		ret = copy_size;
	}

	return ret;

err_1:
	if (image_data[tmp_vbuf.index] != NULL) {
		munmap(image_data[tmp_vbuf.index], tmp_vbuf.length);
		image_data[tmp_vbuf.index] = NULL;
	}

	if (vd > 0) {
		close(vd);
		vd = -1;
	}

	return ret;
}

void exit_camera(void)
{
	int i;

	for (i = 0; i < VBUF_COUNT; i++) {
		if (image_data[i] != NULL) {
			munmap(image_data[i], vbuf[i].length);
			image_data[i] = NULL;
	}
	}

	if (vd > 0) {
		close(vd);
		vd = -1;
	}
}

#if 0
int main(void)
{
	int ret;
	int fd;
	void *yuv_buf;

	yuv_buf = malloc(HEIGHT * WIDTH * 3);

	ret = init_camera(DEV, HEIGHT, WIDTH);
	/* if error */

	ret = get_frame(yuv_buf, HEIGHT * WIDTH * 3);
	/* if error */

	exit_camera();

	fd = open(IMG, O_RDWR |O_CREAT |O_TRUNC, 0644);

	ret = write(fd, yuv_buf, HEIGHT * WIDTH * 3);

	free(yuv_buf);
	close(fd);
	fflush(NULL);

	return ret;
}
#endif


#ifdef __cplusplus
}
#endif

#endif /* linux */
